/*
 * scheduler.c
 *
 * Created: 9/4/2011 12:56:29 PM
 *  Author: GrubyGrub
 */ 

#include <asf.h>
#include <math.h>
#include "ekf.h"
#include "scheduler.h"
#include "conf_scheduler.h"
#include "conf_board.h"
#include "imu_sensors.h"
#include "conf_pdca.h"
#include "usart1_pdca.h"


volatile static unsigned int magacc = 0;
volatile unsigned long millis = 0;
struct marg_sensors_t * marg_s;


/*! \Very primative scheduler, can upgrade to linked list task swtching later
 */
#if (defined __GNUC__)
__attribute__((__interrupt__))
#elif (defined __ICCAVR32__)
#pragma handler = EXAMPLE_TC_IRQ_GROUP, 1
__interrupt
#endif
/*WE ARE ASSUMING INTERRUPTS ABOVE THIS IS LESS THAN 1MS in length.*/
static void sensors_tc_irq(void)
{
	millis++;
	/*100 hz for mag, 500 for mag/acc*/
	switch(magacc)
	{
		//*100 hz
		case 2:
			
			/*dont do anything if it is currently locked*/
			if(marg_s->M_Lock)
				return;
			//printf("number: %d",millis);
			
			
			//load the channel so we can start the DMA*/	 
			pdca_reload_channel(TWI_PDCA_CHANNEL,(void *)marg_s->Mag_buf,READ_LENGTH);
					
			//start the DMA
			marg_s->sensor_t = MAGNETOMETER;	
			twi_master_read_dma(&AVR32_TWI,&(marg_s->Mag_pkt_r));	
		break;
		
		/*read the acclermeter, running at 250HZ*/
		case 10:
			magacc = 0;
		case 0:	
		case 4:
		case 8:
		/*dont do anything if it is currently locked*/
			if(marg_s->A_Lock)
					return;
			
			//load the channel so we can start the DMA*/	 
			pdca_reload_channel(TWI_PDCA_CHANNEL,(void *)marg_s->Acc_buf,READ_LENGTH);
				
			//start the DMA
			marg_s->sensor_t = ACCELEROMETER;
			twi_master_read_dma(&AVR32_TWI,&(marg_s->Acc_pkt_r));	
			break;
		/*read the gyroscope, running at 250HZ*/
		case 1:
		case 5:
		case 9:
			/*dont do anything if it is currently locked*/
			if(marg_s->G_Lock)
				return;
					
			//load the channel so we can start the DMA*/	 
			pdca_reload_channel(TWI_PDCA_CHANNEL,(void *)marg_s->Gyro_buf,READ_LENGTH);
	
			//start the DMA
			marg_s->sensor_t = GYROSCOPE;
			twi_master_read_dma(&AVR32_TWI,&(marg_s->Gyro_pkt_r));
			break;
	}
	
	magacc++;
	
	/*calculate the time for this interrupt, and take it into account..*/
	tc_write_rc(TIMER, SENSOR_TIMER_CHANNEL, SENSOR_TIMER_COUNT - tc_read_tc(TIMER,SENSOR_TIMER_CHANNEL));
	tc_read_sr(TIMER, SENSOR_TIMER_CHANNEL);
	
	return;
	
}

/*init the sensor timers*/
void sensor_scheduler_init(struct marg_sensors_t* marg)
{
	
	
  marg_s = marg;
  twi_master_set_sensor_struct(marg);
  
	printf("scheduler\n");
  // Options for waveform generation.
  static const tc_waveform_opt_t WAVEFORM_OPT =
  {
    .channel  = SENSOR_TIMER_CHANNEL,                        // Channel selection.

    .bswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOB.
    .beevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOB.
    .bcpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOB.
    .bcpb     = TC_EVT_EFFECT_NOOP,                // RB compare effect on TIOB.

    .aswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOA.
    .aeevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOA.
    .acpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOA: toggle.
    .acpa     = TC_EVT_EFFECT_NOOP,                // RA compare effect on TIOA: toggle (other possibilities are none, set and clear).

    .wavsel   = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,// Waveform selection: Up mode with automatic trigger(reset) on RC compare.
    .enetrg   = false,                             // External event trigger enable.
    .eevt     = 0,                                 // External event selection.
    .eevtedg  = TC_SEL_NO_EDGE,                    // External event edge selection.
    .cpcdis   = false,                             // Counter disable when RC compare.
    .cpcstop  = false,                             // Counter clock stopped with RC compare.

    .burst    = false,                             // Burst signal selection.
    .clki     = false,                             // Clock inversion.
    .tcclks   = TC_CLOCK_SOURCE_TC3            // Internal source clock 3, connected to fPBA / 8.
  };

  static const tc_interrupt_t TC_INTERRUPT =
  {
    .etrgs = 0,
    .ldrbs = 0,
    .ldras = 0,
    .cpcs  = 1,
    .cpbs  = 0,
    .cpas  = 0,
    .lovrs = 0,
    .covfs = 0
  };
  
  
  Disable_global_interrupt();
  // Register the RTC interrupt handler to the interrupt controller.
  INTC_register_interrupt(&sensors_tc_irq, SENSOR_TIMER_IRQ_CHANNEL, SENSOR_TIMER_IRQ_LEVEL);
  Enable_global_interrupt();
  
  tc_init_waveform(TIMER, &WAVEFORM_OPT);
  
  /*each second..*/
  tc_write_rc(TIMER, SENSOR_TIMER_CHANNEL, SENSOR_TIMER_COUNT); // Set RC value.
 
  //interrupt at each ms
  tc_configure_interrupts(TIMER, SENSOR_TIMER_CHANNEL, &TC_INTERRUPT);
  
  
}

#if (defined __GNUC__)
__attribute__((__interrupt__))
#elif (defined __ICCAVR32__)
#pragma handler = EXAMPLE_TC_IRQ_GROUP, 1
__interrupt
#endif
/*this interrupt should fire at 250Hz*/
static void print_tc_irq(void)
{
	//PrintEuler();
	PrintQuaternions();
	tc_write_rc(TIMER, PRINT_TIMER_CHANNEL, PRINT_TIMER_COUNT - tc_read_tc(TIMER,PRINT_TIMER_CHANNEL));
	tc_read_sr(TIMER, PRINT_TIMER_CHANNEL);
}

/*intitlized teh ekf scheduler, ekf runs at 250hz below the priority level of the timer*/
void print_scheduler_init(struct marg_sensors_t* marg)
{
	  
  // Options for waveform generation.
  static const tc_waveform_opt_t WAVEFORM_OPT =
  {
    .channel  = PRINT_TIMER_CHANNEL,                        // Channel selection.

    .bswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOB.
    .beevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOB.
    .bcpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOB.
    .bcpb     = TC_EVT_EFFECT_NOOP,                // RB compare effect on TIOB.

    .aswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOA.
    .aeevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOA.
    .acpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOA: toggle.
    .acpa     = TC_EVT_EFFECT_NOOP,                // RA compare effect on TIOA: toggle (other possibilities are none, set and clear).

    .wavsel   = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,// Waveform selection: Up mode with automatic trigger(reset) on RC compare.
    .enetrg   = false,                             // External event trigger enable.
    .eevt     = 0,                                 // External event selection.
    .eevtedg  = TC_SEL_NO_EDGE,                    // External event edge selection.
    .cpcdis   = false,                             // Counter disable when RC compare.
    .cpcstop  = false,                             // Counter clock stopped with RC compare.

    .burst    = false,                             // Burst signal selection.
    .clki     = false,                             // Clock inversion.
    .tcclks   = TC_CLOCK_SOURCE_TC5            // Internal source clock 3, connected to fPBA / 8.
  };

  static const tc_interrupt_t TC_INTERRUPT =
  {
    .etrgs = 0,
    .ldrbs = 0,
    .ldras = 0,
    .cpcs  = 1,
    .cpbs  = 0,
    .cpas  = 0,
    .lovrs = 0,
    .covfs = 0
  };
  
  
  Disable_global_interrupt();
  // Register the RTC interrupt handler to the interrupt controller.
  INTC_register_interrupt(&print_tc_irq, PRINT_TIMER_IRQ_CHANNEL, PRINT_TIMER_IRQ_LEVEL);
  Enable_global_interrupt();
  
  tc_init_waveform(TIMER, &WAVEFORM_OPT);
  
  /*each second..*/
  tc_write_rc(TIMER, PRINT_TIMER_CHANNEL, PRINT_TIMER_COUNT); // Set RC value.
 
  //interrupt at each ms
  tc_configure_interrupts(TIMER, PRINT_TIMER_CHANNEL, &TC_INTERRUPT);
  
  
}

void start_schedulers()
{
	 //starting..
  tc_start(TIMER,SENSOR_TIMER_CHANNEL);
  
  //starting.. 
  tc_start(TIMER,PRINT_TIMER_CHANNEL);
  
}